#! /usr/bin/env python
"""
    这是手动控制的接口，接收上面传下来的信息，控制手臂运动和车运动两个线程向下发送信息
"""
#1.导包 
import rospy
import sys
sys.path.append("/home/galm/ms_01/src/motion_parser/scripts")
from command_handler.msg import manual_cmd
from manualThreadManager import ManualThreadManager
'''
    订阅到信息之后的回调函数，该函数应该做两件事
    1.解析信息，获取有效内容
    2.将解析到的信息和线程通信
'''
def parseCmd(msg):
    cmdDic = {}
    # print(msg)
    cmdDic["armCmd"] = [msg.armPosYaw,msg.armPosPit]
    cmdDic["carCmd"] = [msg.carVelLin,msg.carVelRot]
    cmdDic["shoot"] = msg.shoot
    return cmdDic

def doMsg(msg):
    # rospy.loginfo("I heard:%s",msg)
    res = parseCmd(msg)
    manager.updateArm(res["armCmd"])
    manager.updateCar(res["carCmd"])
    if(res["shoot"]):
        #如果按下射击键才会发布射击命令
        manager.updateShoot(str(res["shoot"]))

if __name__ == "__main__":
    #1.生成线程管理器，启动线程
    manager = ManualThreadManager()
    #2.初始化 ROS 节点:命名(唯一)  ManualInterfaceName: manual_cmd_parser
    rospy.init_node(rospy.get_param("ManualInterfaceName","manual_cmd_parse"))
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber(rospy.get_param("ManualTopic","manual_cmd"),manual_cmd,doMsg,queue_size=10)
    #4.处理订阅的消息(回调函数)
    #5.设置循环调用回调函数
    rospy.spin()

